#include "roscommunicate.h"
//#include <QDebug>

#include "vehicle/vehicledefine.h"


RosCommunicate::RosCommunicate(string remoteServerAddressIn)
{

    //qDebug()<<"init RosCommunicate";
    soap_init(&client_soap);
    client_soap.connect_timeout = 3;
    client_soap.send_timeout = 3;//超时判定
    client_soap.recv_timeout = 3;
    soap_set_mode(&client_soap,SOAP_C_UTFSTRING);//UTF-8

    remoteServerAddress=remoteServerAddressIn;
//    remoteServerAddress=remoteServerAddressIn;
//    remoteServerAddress ="127.0.0.1:5080";
//    isInitialStatus=false;
    isInitialStatus=true;





}

//int RosCommunicate::feedRosWatchDog(soap *, UserInfo userInfo, int flagIn, int &returnFlag)
//{
//    int returnResult;
//    if(soap_call_feedRosWatchDog(&client_soap,remoteServerAddress.c_str(),"",
//                             userInfo,flagIn,returnFlag) == SOAP_OK)
//    {
////        qDebug()<<"feedRosWatchDog sucess!";
//        returnResult= 1;
//    }
//    else
//    {
////        qDebug()<<"feedRosWatchDog error!";
//        returnResult= -1;
//    }
//    soap_destroy(&client_soap);
//    soap_end(&client_soap);
//    soap_done(&client_soap);
//    return returnResult;
//}

int RosCommunicate::changeRemoteServerAddress(string remoteServerAddressIn)
{
    remoteServerAddress=remoteServerAddressIn;
    return 1;
}

int RosCommunicate::getWheelMoveSpeed(hmigsoapnamespace::UserInfo userInfo, int robotIdIn, vector<double> &wheelMoveSpeed)
{
    int returnResult;
    if(soap_call_getWheelMoveSpeed(&client_soap,remoteServerAddress.c_str(),"",
                             userInfo,robotIdIn,wheelMoveSpeed) == SOAP_OK)
    {
        //qDebug()<<"getWheelMoveSpeed sucess!";
        returnResult= 1;
    }
    else
    {
        //qDebug()<<"getWheelMoveSpeed error!";
        returnResult= -1;
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    return returnResult;
}

int RosCommunicate::getWheelPathLength(hmigsoapnamespace::UserInfo userInfo, int robotIdIn, vector<double> &wheelPathLength)
{
    int returnResult;
    if(soap_call_getWheelPathLength(&client_soap,remoteServerAddress.c_str(),"",
                             userInfo,robotIdIn,wheelPathLength) == SOAP_OK)
    {
        //qDebug()<<"getWheelPathLength sucess!";
        returnResult= 1;
    }
    else
    {
        //qDebug()<<"getWheelPathLength error!";
        returnResult= -1;
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    return returnResult;
}


int RosCommunicate::getOdometer(hmigsoapnamespace::UserInfo userInfo, int robotIdIn, hmigsoapnamespace::OdemeterData &odemeterDataReturn)
{
    int returnResult;
    if(soap_call_getOdometer(&client_soap,remoteServerAddress.c_str(),"",
                             userInfo,robotIdIn,odemeterDataReturn) == SOAP_OK)
    {
        //qDebug()<<"getOdometer sucess!";
        returnResult= 1;
    }
    else
    {
        //qDebug()<<"getOdometer error!";
        returnResult= -1;
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    return returnResult;
}

int RosCommunicate::vehicleAuto(hmigsoapnamespace::UserInfo userInfo, int robotIdIn,
                                double xMove, double yMove, double rotate, int &returnFlag)
{
    int returnResult;

    if(soap_call_vehicleAuto(&client_soap,remoteServerAddress.c_str(),"",
                             userInfo,robotIdIn,xMove,yMove,rotate,returnFlag) == SOAP_OK)
    {
        //qDebug()<<"vehicleAuto sucess!";
        returnResult= 1;
    }
    else
    {
        //qDebug()<<"vehicleAuto error!";
        returnResult= -1;
    }
    soap_destroy(&client_soap);
    soap_end(&client_soap);
    soap_done(&client_soap);
    return returnResult;
}





